#include "find_state_error.h"
#include "housekeeping_internal.h"
#include <string.h>
#include <math.h>
#include "system.h"
#include "math_matrix.h"
#include "ZRGame.h"
#include "ZRGameInternal.h"
#include "ZR_API_internal.h"
#include "ZR_API.h"
#include "gsp.h"

/* ================================
 *			ZR Game File
 * ================================
 *
 * The contents of this file are completely up to the game designer.  Feel free
 * to add functions or variables that will be useful in scoring the game.
 *
 */

/*
 * ZR_TIMEOUT
 *
 * Total time of the ZR game in milliseconds.  After ZR_TIMEOUT the game will end.
 */
#define ZR_TIMEOUT 210000

// item collection:
static unsigned char hasRepulsor;
static unsigned char otherHasRepulsor;
static unsigned char hasMagnet;
static unsigned char otherHasMagnet;
static unsigned char hasSRepulsor;
static unsigned char otherHasSRepulsor;
static unsigned char hasSMagnet;
static unsigned char otherHasSMagnet;
static unsigned char hasShield;
static unsigned char otherHasShield;
static unsigned char hasLaser;
static unsigned char otherHasLaser;
static unsigned char hasBHG;
static unsigned char otherHasBHG;
static unsigned char hasEMP;
static unsigned char otherHasEMP;

static unsigned char shieldOnBHG; // 1 if the shield has not yet been broken
static unsigned char shieldOnEMP;

// hill+scoring stuff
static unsigned char onWeakHill;
static unsigned char otherOnWeakHill;
static unsigned char spinningOnWeakHill;
static unsigned char otherSpinningOnWeakHill;

static unsigned char shootingStrongHill;
static unsigned char otherShootingStrongHill;

static int hillHPRemaining;
static unsigned char hillShieldDestroyed; // can't gain points on strong hill till shield is down
static unsigned char hillAuraDestroyed; // can't fire laser from outside XX radius until this aura is down

static unsigned char onStrongHill;
static unsigned char otherOnStrongHill;
static unsigned char spinningOnStrongHill;
static unsigned char otherSpinningOnStrongHill;

static int currentScore;
static int otherCurrentScore;

// fuel variables
static unsigned int fuelEmpty = 0;
static unsigned int otherFuelEmpty = 0;
static float fuelRemaining;

// item use variables
static unsigned int numberRepelsRemaining;
static unsigned int numberMagnetsRemaining;
static unsigned int numberShieldsRemaining;

static unsigned char firingRepulsor;      //not quite sure if we really need all of these; maybe just for lazer
static unsigned char otherFiringRepulsor;  // repulsor and magnet.
static unsigned char firingMagnet;
static unsigned char otherFiringMagnet;
static unsigned char firingSRepulsor;
static unsigned char otherFiringSRepulsor;
static unsigned char firingSMagnet;
static unsigned char otherFiringSMagnet;
static unsigned char losingShield;
static unsigned char otherLosingShield;
static unsigned char firingLaser;
static unsigned char otherFiringLaser;
static unsigned char firingBHG;
static unsigned char otherFiringBHG;
static unsigned char firingEMP;
static unsigned char otherFiringEMP;

static unsigned short userSetRepelForce = 0;
static unsigned short otherSetRepelForce = 0;
const float repelForce = 1.0;
const float repelDrainRate = 10.0f;  //How many units of charge are used up per second when repelling
static unsigned char repelDisabled;



//NOTE: THESE VARIABLES MUST REMAIN GLOBALLY VISIBLE TO BE SET EXTERNALLY BY THE SIMULATION
#ifdef ZRSIMULATION

#else

#endif

float omega = (2*PI)/30.0f;

/*
 * initZRGame
 *
 * Sets up variables used in the ZERO Robotics game.
 *
 */
void initZRGame()
{
#ifndef ZRSIMULATION //User-configurable in the simulation
	//havePanel = 0;
#endif
	//#TODO: initialize varibles here
	hasRepulsor = 1;
	otherHasRepulsor = 1;
	hasMagnet = 1;
	otherHasMagnet = 1;
	hasSRepulsor = 0;
	otherHasSRepulsor = 0;
	hasSMagnet = 0;
	otherHasSMagnet=0;
	hasShield = 0;
	otherHasShield = 0;
	hasLaser = 0;
	otherHasLaser = 0;
	hasBHG = 0;
	otherHasBHG = 0;
	hasEMP = 0;
	otherHasEMP = 0;
	
	shieldOnBHG = 1;
	shieldOnEMP = 1;

	onWeakHill = 0;
	otherOnWeakHill = 0;
	spinningOnWeakHill = 0;
	otherSpinningOnWeakHill = 0;
	
	onStrongHill = 0;
	otherOnStrongHill = 0;
	spinningOnStrongHill = 0;
	otherSpinningOnStrongHill = 0;
	
	hillHPRemaining = 50;
	hillShieldDestroyed = 0;
	hillAuraDestroyed = 0;
	
	currentScore = 0;
	otherCurrentScore = 0;
	
	numberRepelsRemaining = 5;
	numberMagnetsRemaining = 5;
	numberShieldsRemaining = 5;
	
	firingRepulsor = 0;
	otherFiringRepulsor = 0;
	firingSRepulsor = 0;
	otherFiringSRepulsor = 0;
	firingMagnet = 0;
	otherFiringMagnet = 0;
	firingSMagnet = 0;
	otherFiringSMagnet = 0;
	losingShield = 0;
	otherLosingShield = 0;
	firingBHG = 0;
	otherFiringBHG = 0;
	firingEMP = 0;
	otherFiringEMP = 0;
	
	repelDisabled = 0;
}

/*
 * updateZRGame
 *
 * Runs at every control cycle to update the score and game details.
 *
 */
void updateZRGame(unsigned int maneuver_time, state_vector ctrlStateMe, state_vector ctrlStateOther, int ZRtimeout)
{
	// =============== Item Acquisition =============== //
	if (spinComplete == 1) {
		//for each item, check distance from 
		//if (closeEnoughToSRepulsor) {
			//hasSRepulsor = 1;
		//}
	}
	
	// =============== Item Usage =============== //
	// also make sure to check direction is ok for direction based items
	// e.g., laser, magnet, repel.
	if (firingRepulsor) {
		numberRepelsRemaining--;
		if (numberRepelsRemaining >=0) {
			numberRepelsRemaining+=0; // dummy line of code; just here as a placeholder
			//actually should do repulsor's effect here
		}
	}
	if (firingLaser) {
		//do laser effect
		
	}
	//etc.
	
	//Clear user repel force
	userSetRepelForce = 0;
	
	// =============== Shield Destruction =============== //
	if (shootingStrongHill) {
		hillHPRemaining--;
	}
	if (otherShootingStrongHill) {
		hillHPRemaining--;
	}
	if (hillHPRemaining <=0) {
		hillShieldDestroyed = 1;
		hillAuraDestroyed = 1;
	}
	
	// =============== Game Boundaries =============== //
	
	// Detect breach of interaction zone
	if (outsideBoundary(ctrlStateMe)){
		//outside boundary -> repel disabled
		repelDisabled = 1;
	} else {
		repelDisabled = 0;
	}
	
	
	// =============== Fuel =============== //
	if(getPercentFuelRemaining() <= 0 && fuelEmpty == 0 )
	{
		fuelEmpty = 1;
	}
	
	// =============== Scoring =============== //
	if (onStrongHill){
		currentScore+=2;
	}
	if (spinningOnStrongHill){
		currentScore+=2;
	}
	if (onWeakHill){
		currentScore++;
	}
	if (spinningOnWeakHill){
		currentScore++;
	}
	if (shootingStrongHill){
		currentScore++;
	}
	
	if (otherOnStrongHill){
		otherCurrentScore+=2;
	}
	if (otherSpinningOnStrongHill){
		otherCurrentScore+=2;
	}
	if (otherOnWeakHill){
		otherCurrentScore++;
	}
	if (otherSpinningOnWeakHill){
		otherCurrentScore++;
	}
	if (otherShootingStrongHill){
		otherCurrentScore++;
	}
	
}



// Return score with respect to this satellite
float getCurrentScore()
{
	return currentScore;
}

// Return score with respect to other satellite
float getOtherCurrentScore()
{
	return otherCurrentScore;
}

// Repulsion Functions
void ZRRepel()
{	/*
	if (charge >= repelDrainRate)
	{
		//Deplete charge in any case the user calls the function
		charge = charge - repelDrainRate;
		if (charge < 0) charge = 0.0f;
		//Only fire repel if not disabled
		if (!repelDisabled) userSetRepelForce = 1;
	} */
}
unsigned short getUserRepelForce()
{
	return userSetRepelForce;
}
void setOtherRepelForce(unsigned short repelForce)
{	
	otherSetRepelForce = repelForce;
}

// the way that heliospheres did repel.  leaving this in here for now in case we want to steal there style;
// else we can simply repurpose the body of this method.
void ZRGetRepelForce(float refVector[3], float *ctrlForces, state_vector ctrlStateMe, state_vector ctrlStateOther)
{
	int i;
	float sphereRelVec[3]; // Sphere to Sphere Vector
	float attVecMe[3]; // Global Frame mySPHERE attitude Vector
	float attVecOther[3]; // Global Frame otherSPHERE attitude Vector
	unsigned char blocking=0;
	
	if (otherSetRepelForce && !outsideBoundary(ctrlStateMe))
	{
		// Assign relative vector: SPHERE to SPHERE
		for (i=0; i<3; i++)
			sphereRelVec[i] = ctrlStateOther[i] - ctrlStateMe[i];
		//Normalize relative vector
		mathVecNormalize(sphereRelVec,3);
		//Retrieve Global SPHERE attitude vector
		ZRQuat2AttVec(refVector,&ctrlStateMe[QUAT_1],attVecMe);
		//Normalize attitude vector (just in case)
		mathVecNormalize(attVecMe,3);
		//Retrieve Global SPHERE attitude vector
		ZRQuat2AttVec(refVector,&ctrlStateOther[QUAT_1],attVecOther);
		//Normalize attitude vector (just in case)
		mathVecNormalize(attVecOther,3);
		//Check if otherSPHERE's cone of sight is facing me							and mine is not facing it
		if((mathVecInner(sphereRelVec,attVecOther,3) <= -cosf(CONE_ANGLE/2))){
			//Check if I am blocking the repel (ZRRepel is active and my cone is facing other sat
			blocking = userSetRepelForce && (mathVecInner(sphereRelVec,attVecMe,3) >= cosf(CONE_ANGLE/2));
			if (!blocking){
				for(i=0; i<3; i++){
					ctrlForces[i] = -sphereRelVec[i] * (repelForce/dist3d(ctrlStateMe, ctrlStateOther));
				}
			}
		}
	}
}
unsigned char otherRepelling()
{
	if (otherSetRepelForce)
		return 1;
	else
		return 0;
}

/************** FUEL METHODS ****************/

/**
 * Calculates remaining fuel as a percentage of the maximum allowable
 */
float getPercentFuelRemaining()
{
	float fuelRem = 100.0f - 100.0f*((float)hkpTankUseGetZR() - (float)initTankCount)/ (float) maxFuelUse;
	if (fuelRem < 0.0f){
		fuelRem = 0;
	}
	return fuelRem;
}

/**
 * Sets the maximum fuel use to a custom value as a percentage (0 to 1) of a SPHERES tank(e.g. for flat floor testing)
 */
void setMaxFuelUsePercent(float percent){
	maxFuelUse = (int)(TANK_MASS*percent / AVG_MASS_LOSS);  //170g in one tank, teams allowed FUEL_ALLOWED*100%, number converted to tankUse number in thruster*ms
}

int getMaxFuelUse() {
	return maxFuelUse;
}

void setMaxFuel(float fuel){
	FUEL_ALLOWED = fuel;
}

int getFuelFlag()
{
	return fFuelDone;
}

void setOtherFuelFlag(int fuelDone)
{
	otherfFuelDone = fuelDone;
}
int getOtherFuelFlag()
{
	return otherfFuelDone;
}


/************** BOUNDARY METHODS ****************/
unsigned char outsideBoundary(float *ctrlStateMe){
	// Detect breach of interaction zone by satellite
	if (fabs(ctrlStateMe[0]) >= ZONE_pX ||
		fabs(ctrlStateMe[1]) >= ZONE_pY ||
		fabs(ctrlStateMe[2]) >= ZONE_pZ){
		return 1;
	}
	return 0;
}

void enforceBoundaryLimits(state_vector ctrlState, float ctrlControl[6]){
	
	//Run velocity controller to stop motion out of volume
	extern const float KPvelocityP;
	state_vector err;
	int i;
	float limits[3] = {ZONE_pX,ZONE_pY,ZONE_pZ};
	memset(err,0,sizeof(state_vector));
	if (outsideBoundary(ctrlState)){
		//Zero out any controls that are leading the user out of the volume
		for (i=0; i<3; i++){
			if (fabs(ctrlState[POS_X+i]) > limits[i]){
				if(ctrlState[POS_X+i]*ctrlControl[i] > 0){ //If state and control have the same sign
					ctrlControl[i] = 0; //Zero out control
				}
				if (ctrlState[POS_X+i]*ctrlState[VEL_X+i] > 0) { //If velocity and state have same sign
					err[VEL_X+i] = -ctrlState[VEL_X+i]; //Count this as a velocity error
				}
			}
		}
		//Run velocity controller to null out outbound velocities
		ctrlVelocityPgains(KPvelocityP,KPvelocityP,KPvelocityP,err,ctrlControl);
	}
}

/************** GETTERS AND SETTERS ***************/
unsigned char hasRepulsor() {
	return hasRepulsor;
}

void setHasRepulsor(unsigned char value) {
	hasRepulsor = value;
}

unsigned char otherHasRepulsor() {
	return otherHasRepulsor;
}

void setOtherHasRepulsor(unsigned char value) {
	otherHasRepulsor = value;
}

unsigned char hasMagnet() {
	return hasMagnet;
}

void setHasMagnet(unsigned char value) {
	hasMagnet = value;
}

unsigned char otherHasMagnet() {
	return otherHasMagnet;
}

void setOtherHasMagnet(unsigned char value) {
	otherHasMagnet = value;
}

unsigned char hasSRepulsor() {
	return hasSRepulsor;
}

void setHasSRepulsor(unsigned char value) {
	hasSRepulsor = value;
}

unsigned char otherHasSRepulsor() {
	return otherHasSRepulsor;
}

void setOtherHasSRepulsor(unsigned char value) {
	otherHasSRepulsor = value;
}

unsigned char hasSMagnet() {
	return hasSMagnet;
}

void setHasSMagnet(unsigned char value) {
	hasSMagnet = value;
}

unsigned char otherHasSMagnet() {
	return otherHasSMagnet;
}

void setOtherHasSMagnet(unsigned char value) {
	otherHasSMagnet = value;
}

unsigned char hasShield() {
	return hasShield;
}

void setHasShield(unsigned char value) {
	hasShield = value;
}

unsigned char otherHasShield() {
	return otherHasShield;
}

void setOtherHasShield(unsigned char value) {
	otherHasShield = value;
}

unsigned char hasLaser() {
	return hasLaser;
}

void setHasLaser(unsigned char value) {
	hasLaser = value;
}

unsigned char otherHasLaser() {
	return otherHasLaser;
}

void setOtherHasLaser(unsigned char value) {
	otherHasLaser = value;
}

unsigned char hasBHG() {
	return hasBHG;
}

void setHasBHG(unsigned char value) {
	hasBHG = value;
}

unsigned char otherHasBHG() {
	return otherHasBHG;
}

void setOtherHasBHG(unsigned char value) {
	otherHasBHG = value;
}

unsigned char hasEMP() {
	return hasEMP;
}

void setHasEMP(unsigned char value) {
	hasEMP = value;
}

unsigned char otherHasEMP() {
	return otherHasEMP;
}

void setOtherHasEMP(unsigned char value) {
	otherHasEMP = value;
}

unsigned char shieldOnBHG() {
	return shieldOnBHG;
}

void setShieldOnBHG(unsigned char value) {
	shieldOnBHG = value;
}

unsigned char shieldOnEMP() {
	return shieldOnEMP;
}

void setShieldOnEMP(unsigned char value) {
	shieldOnEMP = value;
}

unsigned char onWeakHill() {
	return onWeakHill;
}

void setOnWeakHill(unsigned char value) {
	onWeakHill = value;
}

unsigned char otherOnWeakHill() {
	return otherOnWeakHill;
}

void setOtherOnWeakHill(unsigned char value) {
	otherOnWeakHill = value;
}

unsigned char spinningOnWeakHill() {
	return spinningOnWeakHill;
}

void setSpinningOnWeakHill(unsigned char value) {
	spinningOnWeakHill = value;
}

unsigned char otherSpinningOnWeakHill() {
	return otherSpinningOnWeakHill;
}

void setOtherSpinningOnWeakHill(unsigned char value) {
	otherSpinningOnWeakHill = value;
}

unsigned char shootingStrongHill() {
	return shootingStrongHill;
}

void setShootingStrongHill(unsigned char value) {
	shootingStrongHill = value;
}

unsigned char otherShootingStrongHill() {
	return otherShootingStrongHill;
}

void setOtherShootingStrongHill(unsigned char value) {
	otherShootingStrongHill = value;
}

float hillHPRemaining() {
	return hillHPRemaining;
}

void setHillHPRemaining(float value) {
	hillHPRemaining = value;
}

unsigned char hillShieldDestroyed() {
	return hillShieldDestroyed;
}

void setHillShieldDestroyed(unsigned char value) {
	hillShieldDestroyed = value;
}

unsigned char hillAuraDestroyed() {
	return hillAuraDestroyed;
}

void setHillAuraDestroyed(unsigned char value) {
	hillAuraDestroyed = value;
}

unsigned char onStrongHill() {
	return onStrongHill;
}

void setOnStrongHill(unsigned char value) {
	onStrongHill = value;
}

unsigned char otherOnStrongHill() {
	return otherOnStrongHill;
}

void setOtherOnStrongHill(unsigned char value) {
	otherOnStrongHill = value;
}

unsigned char spinningOnStrongHill() {
	return spinningOnStrongHill;
}

void setSpinningOnStrongHill(unsigned char value) {
	spinningOnStrongHill = value;
}

unsigned char otherSpinningOnStrongHill() {
	return otherSpinningOnStrongHill;
}

void setOtherSpinningOnStrongHill(unsigned char value) {
	otherSpinningOnStrongHill = value;
}

float currentScore() {
	return currentScore;
}

void setCurrentScore(float value) {
	currentScore = value;
}

float otherCurrentScore() {
	return otherCurrentScore;
}

void setOtherCurrentScore(float value) {
	otherCurrentScore = value;
}

unsigned int fuelEmpty() {
	return fuelEmpty;
}

void setFuelEmpty(unsigned int value) {
	fuelEmpty = value;
}

unsigned int otherFuelEmpty() {
	return otherFuelEmpty;
}

void setOtherFuelEmpty(unsigned int value) {
	otherFuelEmpty = value;
}

float fuelRemaining() {
	return fuelRemaining;
}

void setFuelRemaining(float value) {
	fuelRemaining = value;
}

unsigned int numberRepelsRemaining() {
	return numberRepelsRemaining;
}

void setNumberRepelsRemaining(unsigned int value) {
	numberRepelsRemaining = value;
}

unsigned int numberMagnetsRemaining() {
	return numberMagnetsRemaining;
}

void setNumberMagnetsRemaining(unsigned int value) {
	numberMagnetsRemaining = value;
}

unsigned int numberShieldsRemaining() {
	return numberShieldsRemaining;
}

void setNumberShieldsRemaining(unsigned int value) {
	numberShieldsRemaining = value;
}

unsigned char firingRepulsor() {
	return firingRepulsor;
}

void setFiringRepulsor(unsigned char value) {
	firingRepulsor = value;
}

unsigned char otherFiringRepulsor() {
	return otherFiringRepulsor;
}

void setOtherFiringRepulsor(unsigned char value) {
	otherFiringRepulsor = value;
}

unsigned char firingMagnet() {
	return firingMagnet;
}

void setFiringMagnet(unsigned char value) {
	firingMagnet = value;
}

unsigned char otherFiringMagnet() {
	return otherFiringMagnet;
}

void setOtherFiringMagnet(unsigned char value) {
	otherFiringMagnet = value;
}

unsigned char firingSRepulsor() {
	return firingSRepulsor;
}

void setFiringSRepulsor(unsigned char value) {
	firingSRepulsor = value;
}

unsigned char otherFiringSRepulsor() {
	return otherFiringSRepulsor;
}

void setOtherFiringSRepulsor(unsigned char value) {
	otherFiringSRepulsor = value;
}

unsigned char firingSMagnet() {
	return firingSMagnet;
}

void setFiringSMagnet(unsigned char value) {
	firingSMagnet = value;
}

unsigned char otherFiringSMagnet() {
	return otherFiringSMagnet;
}

void setOtherFiringSMagnet(unsigned char value) {
	otherFiringSMagnet = value;
}

unsigned char losingShield() {
	return losingShield;
}

void setLosingShield(unsigned char value) {
	losingShield = value;
}

unsigned char otherLosingShield() {
	return otherLosingShield;
}

void setOtherLosingShield(unsigned char value) {
	otherLosingShield = value;
}

unsigned char firingLaser() {
	return firingLaser;
}

void setFiringLaser(unsigned char value) {
	firingLaser = value;
}

unsigned char otherFiringLaser() {
	return otherFiringLaser;
}

void setOtherFiringLaser(unsigned char value) {
	otherFiringLaser = value;
}

unsigned char firingBHG() {
	return firingBHG;
}

void setFiringBHG(unsigned char value) {
	firingBHG = value;
}

unsigned char otherFiringBHG() {
	return otherFiringBHG;
}

void setOtherFiringBHG(unsigned char value) {
	otherFiringBHG = value;
}

unsigned char firingEMP() {
	return firingEMP;
}

void setFiringEMP(unsigned char value) {
	firingEMP = value;
}

unsigned char otherFiringEMP() {
	return otherFiringEMP;
}

void setOtherFiringEMP(unsigned char value) {
	otherFiringEMP = value;
}

unsigned short userSetRepelForce() {
	return userSetRepelForce;
}

void setUserSetRepelForce(unsigned short value) {
	userSetRepelForce = value;
}

unsigned short otherSetRepelForce() {
	return otherSetRepelForce;
}

void setOtherSetRepelForce(unsigned short value) {
     otherSetRepelForce = value;
}

const float repelForce() {
     return repelForce;
}

void setRepelForce(const float value) {
     repelForce = value;
}

const float repelDrainRate() {
     return repelDrainRate;
}

void setRepelDrainRate(const float value) {
     repelDrainRate = value;
}

unsigned char repelDisabled() {
     return repelDisabled;
}

void setRepelDisabled(unsigned char value) {
     repelDisabled = value;
}